#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "inc/hw_memmap.h"
#include "driverlib/pwm.h"
#include "driverlib/sysctl.h"
#include "driverlib/timer.h"
#include "driverlib/uart.h"
#include "my_cmd_lib.h"
#include "driverlib/gpio.h"
#include "my_funcs.h"
#include "main.h"


cmd_type cmd_Tablearray[] = {
		{"CLC", clear_terminal},
		{"Start", RWFollowWall},
		{"STOP", WheelStop},
		{"PIDS", PIDparasSet},
		{"PIDG", PIDparasGet},
		{"TFWD", testForward},
		{"TUT", testUturn},
};

robot_state current_state = NONELINE;
cmd_Table_type cmd_Table = {cmd_Tablearray, sizeof(cmd_Tablearray)/sizeof(cmd_type)};
bool STOP = 0;


char* fparams_name[] = {"kp", "kP", "ki", "kd", "kD", "ce", "ut", "rt", "ud", "rd",
		"Qg", "Qu", "Ql", "Rv", "d0", "p0"};
char* iparams_name[] = {"lv", "rv", "jn", "jp", "ul", "ur", "rl", "rr",
		"tl", "tu", "kl", "ku"};
double* fparams[] = {&kp, &kpUpper, &ki, &kd, &kdUpper, &accu_err, &uTurn_thresIn, &rTurn_thresIn,
		&uTurn_thresOut, &rTurn_thresOut, &Q_gain, &Q_upper, &Q_lower, &R, &distance_hat0,
		&P0};
uint32_t* iparams[] = {&defaultL_v, &defaultR_v, &deltaPIDnega, &deltaPIDposi,
		&uturnlv, &uturnrv, &rturnlv, &rturnrv, &thinLower, &thinUpper, &thickLower, &thickUpper};


void clear_terminal(void)
{
	UARTSend((uint8_t *)"\033[2J\033[H", UART1_BASE);
}

void WheelStop(void){
	PIDStart = 0;
	GPIOIntDisable(GPIO_PORTA_BASE, GPIO_INT_PIN_3);
	STOP = 1;
	current_state = NONELINE;
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, 1);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, 1);
	GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_3 | GPIO_PIN_2 | GPIO_PIN_1, 0);
	UARTSend((uint8_t *)"\r\nThe car stops moving!\r\n", UART1_BASE);
}

void RWFollowWall(void){
	UARTSend((uint8_t *)"\r\nRight wheel follow the Wall!\r\n", UART1_BASE);
	accu_err = 0;
	pre_err = 0;
	Q = Q_upper;
	P = P0;
	distance_hat = distance_hat0;
	TR = 0;
	UT = 0;
	pingIdx = 0;
	pingDataNum = 0;
	pingpong = 0;
	STOP = 0;
	PIDStart = 1;
	GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_3 | GPIO_PIN_2 | GPIO_PIN_1, 0);
	GPIOIntEnable(GPIO_PORTA_BASE, GPIO_INT_PIN_3);
}

void PIDparasSet(void){
	pid_cmd = 1;
	UARTSend((uint8_t *)"\r\nPlease modify the PID parameters in the following format: kp=2.0\r\n", UART1_BASE);
	while(pid_cmd);
	int num_fparams_name = sizeof(fparams) / sizeof(fparams[0]);
	int num_total = num_fparams_name + sizeof(iparams) / sizeof(iparams[0]);
	int i = 0;
	while (i < num_total){
		if (i < num_fparams_name){
			if (!strncmp(cmd_received, fparams_name[i], 2)){
				*fparams[i] = atof(cmd_received+3);
				break;
			}
		}
		else {
			if (!strncmp(cmd_received, iparams_name[i-num_fparams_name], 2)){
				*iparams[i-num_fparams_name] = atoi(cmd_received+3);
				break;
			}
		}
		i++;
	}
	if (i < num_total){
		PIDparasGet();
	}
	else
	UARTSend((uint8_t *)"\r\nModification failed\r\n", UART1_BASE);
}


void PIDparasGet(void){
	char temp[12];
	if (fabs(accu_err) >= 1000){
		accu_err = accu_err > 0? 999 : -999;
	}
	UARTSend((uint8_t *)"\r\n", UART1_BASE);
	int num = sizeof(fparams) / sizeof(fparams[0]);
	int i = 0;
	while (i < num){
		UARTSend((uint8_t *)fparams_name[i], UART1_BASE);
		UARTCharPut(UART1_BASE, '=');
		double2str(temp, *fparams[i]);
		UARTSend((uint8_t *)temp, UART1_BASE);
		UARTCharPut(UART1_BASE, ',');
		i++;
	}
	num = sizeof(iparams) / sizeof(iparams[0]);
	i = 0;
	while (i < num){
		UARTSend((uint8_t *)iparams_name[i], UART1_BASE);
		UARTCharPut(UART1_BASE, '=');
		uint2str(temp, *iparams[i]);
		UARTSend((uint8_t *)temp, UART1_BASE);
		UARTCharPut(UART1_BASE, ',');
		i++;
	}
	UARTSend((uint8_t *)"\b \r\n", UART1_BASE);
}


void testForward(void){
	UARTSend((uint8_t *)"\r\nTest forward!\r\n", UART1_BASE);
	GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_3 | GPIO_PIN_2, GPIOD_F);
	GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_7 | GPIO_PIN_6, GPIOA_F);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, defaultL_v);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, defaultR_v);
}


void testUturn(void){
	UARTSend((uint8_t *)"\r\nTest U-Turn!\r\n", UART1_BASE);
	GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_3 | GPIO_PIN_2, GPIOD_F);
	GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_7 | GPIO_PIN_6, GPIOA_B);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, uturnlv);
	PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, uturnrv);
}
